#include <internal.h>
#include <pcl/common/time.h>

#ifndef TSDF_BUFFER_H_
#define TSDF_BUFFER_H_

namespace pcl
{
  namespace device
  {

    //////////////////////////////////////////////
    //VARIABLES

    
    //////////////////////////////////////////////
    //ACCESSORS AND HELPER FUNCTIONS
    static void computeShiftingBounds(tsdf_buffer *buffer_)
    {      
      //compute shifting thresholds, based on indices values
      float sizeVoxel_x = VOLUME_SIZE/VOLUME_X;
      float sizeVoxel_y = VOLUME_SIZE/VOLUME_Y;
      float sizeVoxel_z = VOLUME_SIZE/VOLUME_Z;
      
      buffer_->shiftingThresholds_min_m.x = buffer_->origin_metric.x + SHIFTING_THRESHOLD*sizeVoxel_x;
      buffer_->shiftingThresholds_max_m.x = buffer_->origin_metric.x + VOLUME_SIZE - SHIFTING_THRESHOLD*sizeVoxel_x;
      
      buffer_->shiftingThresholds_min_m.y = buffer_->origin_metric.y + SHIFTING_THRESHOLD*sizeVoxel_y;
      buffer_->shiftingThresholds_max_m.y = buffer_->origin_metric.y + VOLUME_SIZE - SHIFTING_THRESHOLD*sizeVoxel_y;
      
      buffer_->shiftingThresholds_min_m.z = buffer_->origin_metric.z + SHIFTING_THRESHOLD*sizeVoxel_z;
      buffer_->shiftingThresholds_max_m.z = buffer_->origin_metric.z + VOLUME_SIZE - SHIFTING_THRESHOLD*sizeVoxel_z;
    }
    
    
    static void set_cube_origin_grID(tsdf_buffer *buffer_, int3 new_origin_grID) 
    { 
      buffer_->origin_GRID.x = new_origin_grID.x;  
      buffer_->origin_GRID.y = new_origin_grID.y; 
      buffer_->origin_GRID.z = new_origin_grID.z;
    }
    
    inline void set_cube_origin_meters(tsdf_buffer *buffer_, float3 new_origin_meters) 
    { 
      buffer_->origin_metric = new_origin_meters;
      //update shifting bounds
      computeShiftingBounds(buffer_);
    }
    
   
    
    inline float3 get_cube_origin_meters(tsdf_buffer *buffer_) { return buffer_->origin_metric; }
    inline int3 get_cube_origin_grID(tsdf_buffer *buffer_)  { return buffer_->origin_GRID; }

  
    // shift given a new camera pose. Compute new cube origin in meters, and shifting offsets in indices
    inline void compute_new_cube_position(tsdf_buffer *buffer_, Eigen::Affine3f camera_pose, pcl::PointXYZ projected_point, int &shiftX, int &shiftY, int &shiftZ)
    {
      //ScopeTime time("\t[tsdf_buffer.h](compute_new_cube_position)");
      //compute new origin for the cube
      float3 new_cube_origin_meters;
      new_cube_origin_meters.x = camera_pose.translation()[0] - VOLUME_SIZE/2.0;
      new_cube_origin_meters.y = camera_pose.translation()[1] -  VOLUME_SIZE/2.0;
      new_cube_origin_meters.z = camera_pose.translation()[2] + 0.1*VOLUME_SIZE;
      
      ///6DOF: Problems with new cube. TODO
      //~ new_cube_origin_meters.x = projected_point.x - VOLUME_SIZE/2.0;
      //~ new_cube_origin_meters.y = projected_point.y -  VOLUME_SIZE/2.0;
      //~ new_cube_origin_meters.z = projected_point.z + 0.1*VOLUME_SIZE;
      
      //~ std::cout << "Projected point: " << projected_point.x << " " << projected_point.y << " " << projected_point.z << std::endl;
      //~ std::cout << "New Origin point: " << new_cube_origin_meters.x << " " << new_cube_origin_meters.y << " " << new_cube_origin_meters.z << std::endl;
      //~ std::cout << "Cur Origin point: " << buffer_->origin_metric.x << " " << buffer_->origin_metric.y << " " << buffer_->origin_metric.z << std::endl;
      
      shiftX = (int)( (new_cube_origin_meters.x - buffer_->origin_metric.x) * ( ((float) VOLUME_X)/VOLUME_SIZE) );
      shiftY = (int)( (new_cube_origin_meters.y - buffer_->origin_metric.y) * ( ((float) VOLUME_Y)/VOLUME_SIZE) );
      shiftZ = (int)( (new_cube_origin_meters.z - buffer_->origin_metric.z) * ( ((float) VOLUME_Z)/VOLUME_SIZE) );
     
      //save the new origin and compute new metric bounds for later shifts
      set_cube_origin_meters(buffer_, new_cube_origin_meters);

    }
    
    // shift given a new Target Point. Compute new cube origin in meters, and shifting offsets in indices
    inline void compute_new_cube_position(tsdf_buffer *buffer_, pcl::PointXYZ target_point, int &shiftX, int &shiftY, int &shiftZ)
    {
      
      
      
      //ScopeTime time("\t[tsdf_buffer.h](compute_new_cube_position)");
      //compute new origin for the cube
      float3 new_cube_origin_meters;
      new_cube_origin_meters.x = target_point.x - VOLUME_SIZE/2.0;
      new_cube_origin_meters.y = target_point.y - VOLUME_SIZE/2.0;
      new_cube_origin_meters.z = target_point.z - VOLUME_SIZE/2.0;
      
      ///6DOF: Problems with new cube. TODO
      //~ new_cube_origin_meters.x = projected_point.x - VOLUME_SIZE/2.0;
      //~ new_cube_origin_meters.y = projected_point.y -  VOLUME_SIZE/2.0;
      //~ new_cube_origin_meters.z = projected_point.z + 0.1*VOLUME_SIZE;
      
      //~ std::cout << "Projected point: " << projected_point.x << " " << projected_point.y << " " << projected_point.z << std::endl;
      std::cout << "New Origin point: " << new_cube_origin_meters.x << " " << new_cube_origin_meters.y << " " << new_cube_origin_meters.z << std::endl;
      //~ std::cout << "Cur Origin point: " << buffer_->origin_metric.x << " " << buffer_->origin_metric.y << " " << buffer_->origin_metric.z << std::endl;
      
      shiftX = (int)( (new_cube_origin_meters.x - buffer_->origin_metric.x) * ( ((float) VOLUME_X)/VOLUME_SIZE) );
      shiftY = (int)( (new_cube_origin_meters.y - buffer_->origin_metric.y) * ( ((float) VOLUME_Y)/VOLUME_SIZE) );
      shiftZ = (int)( (new_cube_origin_meters.z - buffer_->origin_metric.z) * ( ((float) VOLUME_Z)/VOLUME_SIZE) );
     
      //save the new origin and compute new metric bounds for later shifts
      set_cube_origin_meters(buffer_, new_cube_origin_meters);

    }
    

  
    // shift volume by X, Y, Z indices. The shift updates buffer addresses.
    inline void shift_volume(tsdf_buffer *buffer_, pcl::gpu::TsdfVolume::Ptr tsdf_volume, int shiftX, int shiftY, int shiftZ)
    {
      ScopeTime time("\t[tsdf_buffer.h](shift_volume)");

      int3 newOrigin, oldOrigin, shift;
      
      oldOrigin = get_cube_origin_grID(buffer_);

      shift.x = shiftX;
      shift.y = shiftY;
      shift.z = shiftZ; 
      
      //compute new ID origin (modulo size)      
      newOrigin.x = shift.x + oldOrigin.x;
      newOrigin.y = shift.y + oldOrigin.y;
      newOrigin.z = shift.z + oldOrigin.z;
      
      //make sure we are not out of bounds
        if(newOrigin.x >= VOLUME_X)
          newOrigin.x -= VOLUME_X;
        
        if(newOrigin.x < 0)
           newOrigin.x += VOLUME_X;
        
        if(newOrigin.y >= VOLUME_Y)
          newOrigin.y -= VOLUME_Y;
        
        if(newOrigin.y < 0)
           newOrigin.y += VOLUME_Y;
           
        if(newOrigin.z >= VOLUME_Z)
          newOrigin.z -= VOLUME_Z;
        
        if(newOrigin.z < 0)
           newOrigin.z += VOLUME_Z;
           
        std::cout << "New origin indices: " << newOrigin.x << " " << newOrigin.y << " " << newOrigin.z << std::endl;
        
        //cast volume to a tsdf_volume-readable format
        PtrStep<short2> localVolume = tsdf_volume->data();
        
       
        //update the origin indexes
        set_cube_origin_grID(buffer_, newOrigin);
        
        //get memory address of the different buffer elements
        buffer_->tsdf_rolling_buff_origin = &(localVolume.ptr (VOLUME_Y * (newOrigin.z) + (newOrigin.y) )[newOrigin.x]);
        buffer_->tsdf_memory_start = &(localVolume.ptr (0)[0]);
        //std::cout << "Memory chunk first data element " << buffer_->tsdf_memory_start << std::endl;
        buffer_->tsdf_memory_end = &(localVolume.ptr (VOLUME_Y * (VOLUME_Z - 1) + (VOLUME_Y - 1) )[VOLUME_X - 1]);
        //std::cout << "Memory chunk last data element " << buffer_->tsdf_memory_end << std::endl;
  
      }
      
      
      
    // initialize the buffer (<=> reset)
    inline void init_buffer(tsdf_buffer *buffer_, pcl::gpu::TsdfVolume::Ptr tsdf_volume)
    {
      std::cout << "\nRe initializing tsdf buffer..." << std::endl;
      PtrStep<short2> localVolume = tsdf_volume->data();
      
      buffer_->tsdf_memory_start = &(localVolume.ptr (0)[0]);
      buffer_->tsdf_memory_end = &(localVolume.ptr (VOLUME_Y * (VOLUME_Z - 1) + (VOLUME_Y - 1) )[VOLUME_X - 1]);
      buffer_->tsdf_rolling_buff_origin = buffer_->tsdf_memory_start;
      
      buffer_->origin_GRID.x = 0 ;  buffer_->origin_GRID.y = 0 ;  buffer_->origin_GRID.z = 0 ; 
      buffer_->origin_metric.x = 0.0; buffer_->origin_metric.y = 0.0; buffer_->origin_metric.z = 0.0; 
      
      shift_volume(buffer_, tsdf_volume, 0,0,0);
         
      computeShiftingBounds(buffer_);
    
      
      std::cout << "\tDone." << std::endl;
    }
      


  }
}

#endif
    
